import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node # <-- 导入 Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

    # 1. 硬件接口 (保留)
    #    启动 so101_hw_interface 包中的 so101_hw.launch.py
    hw_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare('so101_hw_interface'),
                'launch',
                'so101_hw.launch.py'
            ])
        )
    )

    # 2. ZMQ 控制节点 (新增)
    #    直接启动 joystick_alphadog_with_so101_servo 包中的 ledog_zmq_host 可执行文件
    #    (该可执行文件在 setup.py 中被命名为 'ledog_zmq_host')
    zmq_host_node = Node(
        package='joystick_alphadog_with_so101_servo',
        executable='ledog_zmq_host', # <-- 这是 setup.py 中 entry_points 定义的名字
        name='ledog_zmq_host_node',  # <-- ROS 图中的节点名
        output='screen',
    )
    
    # 3. MoveIt 节点 (移除) - 不再包含 moveit_launch
    
    # 4. 手柄控制 (移除) - 不再包含 controller_launch

    # 将需要启动的节点/Launch文件组合起来
    return LaunchDescription([
        hw_launch,
        zmq_host_node
        # 在这里还可以添加您需要同时启动的其他节点，
        # 例如摄像头驱动节点、机器狗驱动节点 (如果它们没有被 hw_launch 包含的话)
    ])